#include "common.h"

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/SVD>

// 问题1：已知6个控制点在世界坐标系下的坐标和像素坐标，求投影矩阵P
// 利用 DLT 法，注意需要考虑归一化
// s.a. Hartley, R., Zisserman, A., 2004. Multiple View Geometry in Computer Vision, Second. ed. Cambridge University
// Press. 第四章、第七章

namespace pcv {

Matrix34d student_P(const std::vector<Vector3d> &points3d, const std::vector<Vector2d> points2d) {
    Matrix34d P = Matrix34d::Zero();

    return P;
}

Matrix34d golden_P(const std::vector<Vector3d> &points3d, const std::vector<Vector2d> points2d) {
    Matrix34d P;
    P << 801.038489657639, 37.6687425320979, 396.129276781026, -80103.8489657639, -37.5511199810359, 797.079549525369,
        404.047157045567, 3755.11199810359, 0.00507119865602733, -0.00482615167464816, 0.999975495301862,
        -0.507119865602733;

    return P;
}

} // namespace pcv